{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Image to ground\n",
    "\n",
    "## by Kaj Williams and Jesse Mapel\n",
    "## Notes: includes rotation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 94,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from __future__ import print_function, division"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 95,
   "metadata": {},
   "outputs": [],
   "source": [
    "Samples = 7.5\n",
    "Lines = 7.5\n",
    "\n",
    "# optical center (pixels) in x,y direction\n",
    "Cx=7.5  \n",
    "Cy=7.5  \n",
    "\n",
    "# focal length (m)\n",
    "F=50.0e-3 \n",
    "\n",
    "# size of pixels in world units (m)\n",
    "Px=1.0e-4 \n",
    "Py=1.0e-4\n",
    "\n",
    "# observer position:\n",
    "obs_x=1000.0\n",
    "obs_y=0.0\n",
    "obs_z=0.0\n",
    "\n",
    "# radius of body (m):\n",
    "major_radius=10.0\n",
    "minor_radius=10.0\n",
    "\n",
    "#Rotation:\n",
    "omega=0\n",
    "phi=-np.pi/2.\n",
    "kappa=np.pi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Calculation of camera look vector:\n",
    "$\\begin{bmatrix}\\mathbf{x} & \\mathbf{y} & \\mathbf{z} \\end{bmatrix}= \n",
    "\\begin{bmatrix}\\mathbf{Samples} & \\mathbf{Lines} & \\mathbf{1} \\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "\\mathbf{P_y} & \\mathbf{0} & \\mathbf{0}\\\\\n",
    "\\mathbf{0} & \\mathbf{P_x} & \\mathbf{0}\\\\\n",
    "\\mathbf{-C_y P_y} & \\mathbf{-C_x P_x} & \\mathbf{F} \n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "\\mathbf{sin(\\phi)} & \\mathbf{-sin(\\omega)cos(\\phi)} & \\mathbf{cos(\\omega)cos(\\phi)}\\\\\n",
    "\\mathbf{-cos(\\phi)sin(\\kappa)} & \\mathbf{cos(\\omega)cos(\\kappa)-sin(\\omega)sin(\\phi)sin(\\kappa)} & \\mathbf{cos(\\omega)cos(\\kappa)+sin(\\omega)sin(\\phi)sin(\\kappa)}\\\\\n",
    "\\mathbf{cos(\\phi)cos(\\omega)} & \\mathbf{cos(\\omega)sin(\\kappa)+sin(\\omega)sin(\\phi)cos(\\kappa)} & \\mathbf{cos(\\omega)sin(\\kappa)-sin(\\omega)sin(\\phi)cos(\\kappa)} \n",
    "\\end{bmatrix}\n",
    "$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "metadata": {},
   "outputs": [],
   "source": [
    "image_vector = np.array([Samples, Lines, 1], dtype=float)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 97,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera_array = np.array([[Py, 0, 0],[0, Px,0],[-Cy*Py,-Cx*Px,F]], dtype=float)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 98,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Compute the camera look vector and normalize it:\n",
    "camera_look_vector = np.matmul(np.transpose(image_vector),camera_array)\n",
    "camera_look_vector=camera_look_vector/np.linalg.norm(camera_look_vector,2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 99,
   "metadata": {},
   "outputs": [],
   "source": [
    "rotation_matrix=np.ndarray(shape=(3,3), dtype=float, order='F')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 100,
   "metadata": {},
   "outputs": [],
   "source": [
    "rotation_matrix[0,0]=np.cos(phi)*np.cos(kappa)\n",
    "rotation_matrix[1,0]=np.cos(omega)*np.sin(kappa)+np.sin(omega)*np.sin(phi)*np.cos(kappa)\n",
    "rotation_matrix[2,0]=np.sin(omega)*np.sin(kappa)-np.cos(omega)*np.sin(phi)*np.cos(kappa)\n",
    "rotation_matrix[0,1]=-np.cos(phi)*np.sin(kappa)\n",
    "rotation_matrix[1,1]=np.cos(omega)*np.cos(kappa)-np.sin(omega)*np.sin(phi)*np.sin(kappa)\n",
    "rotation_matrix[2,1]=np.sin(omega)*np.cos(kappa)+np.cos(omega)*np.sin(phi)*np.sin(kappa)\n",
    "rotation_matrix[0,2]=np.sin(phi)\n",
    "rotation_matrix[1,2]=-np.sin(omega)*np.cos(phi)\n",
    "rotation_matrix[2,2]=np.cos(omega)*np.cos(phi)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 101,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-6.12323400e-17 -7.49879891e-33 -1.00000000e+00]\n",
      " [ 1.22464680e-16 -1.00000000e+00 -0.00000000e+00]\n",
      " [-1.00000000e+00 -1.22464680e-16  6.12323400e-17]]\n"
     ]
    }
   ],
   "source": [
    "print(rotation_matrix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 102,
   "metadata": {},
   "outputs": [],
   "source": [
    "look_vector=np.matmul(np.transpose(camera_look_vector),rotation_matrix)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 103,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "After accounting for rotation,  look vector x,y,z:  [-1.0000000e+00 -1.2246468e-16  6.1232340e-17]\n"
     ]
    }
   ],
   "source": [
    "print(\"After accounting for rotation,  look vector x,y,z: \",look_vector)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 104,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "a:1.0 b:-2000.0 c:999900.0 radius_squared_ratio:1.0\n"
     ]
    }
   ],
   "source": [
    "radius_squared_ratio =major_radius**2/minor_radius**2\n",
    "a=look_vector[0]**2 + look_vector[1]**2 + radius_squared_ratio*look_vector[2]**2\n",
    "b=2*(look_vector[0]*obs_x+look_vector[1]*obs_y+radius_squared_ratio*look_vector[2]*obs_z)\n",
    "c=obs_x**2+obs_y**2+radius_squared_ratio*obs_z**2-major_radius**2\n",
    "discriminant=b**2-4.0*a*c\n",
    "print('a:{} b:{} c:{} radius_squared_ratio:{}'.format(a,b,c,radius_squared_ratio))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 105,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "400.0"
      ]
     },
     "execution_count": 105,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "if discriminant<0 :\n",
    "    discriminant=0 # closest intersection\n",
    "discriminant"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 106,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "990.0"
      ]
     },
     "execution_count": 106,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "distance=(-b-np.sqrt(discriminant))/(2*a)\n",
    "distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {},
   "outputs": [],
   "source": [
    "obs_vector=np.array([obs_x, obs_y, obs_z])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 108,
   "metadata": {},
   "outputs": [],
   "source": [
    "ground_point = obs_vector+distance*look_vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 109,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Planet coords. x,y,z:  [ 1.00000000e+01 -1.21240033e-13  6.06200166e-14]\n"
     ]
    }
   ],
   "source": [
    "print(\"Planet coords. x,y,z: \",ground_point)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Ground to Image\n",
    "### Notes: very basic implementation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Look vector:\n",
    "x=0\n",
    "y=0\n",
    "z=-1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$\\begin{bmatrix}\\mathbf{Samples} & \\mathbf{Lines} & \\mathbf{1} \\end{bmatrix}=\n",
    "\\begin{bmatrix}\\mathbf{x} & \\mathbf{y} & \\mathbf{z}& \\mathbf{1} \\end{bmatrix} \n",
    "\\begin{bmatrix}\n",
    "\\mathbf{0} & \\mathbf{1/P_x} & \\mathbf{0}\\\\\n",
    "\\mathbf{1/P_y} & \\mathbf{0} & \\mathbf{0}\\\\\n",
    "\\mathbf{0} & \\mathbf{0} & \\mathbf{0}\\\\\n",
    "\\mathbf{C_y} & \\mathbf{C_x} & \\mathbf{1} \n",
    "\\end{bmatrix}\n",
    "$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 111,
   "metadata": {},
   "outputs": [],
   "source": [
    "ground_vector=np.array([x, y, z,1], dtype=float)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 112,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera_array = np.array([[0, 1/Px, 0],[1/Py, 0,0],[0,0,0],[Cy,Cx,1]], dtype=float)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 113,
   "metadata": {},
   "outputs": [],
   "source": [
    "Image_coords = np.matmul(np.transpose(ground_vector),camera_array)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 114,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Image coords. x,y,z:  [7.5 7.5 1. ]\n"
     ]
    }
   ],
   "source": [
    "print(\"Image coords. x,y,z: \",Image_coords)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Radial Distortion Model\n",
    "\n",
    "### The following code is for the Brown-Conrady model, implemented as the \"division model\" approach:\n",
    "### https://en.wikipedia.org/wiki/Distortion_(optics)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 115,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Radial distortion coefficients:\n",
    "# if K1>0 then pincushion distortion.\n",
    "# If K1<0 then barrel distortion.\n",
    "K1=0.1\n",
    "K2=0.0\n",
    "\n",
    "# Distortion center x,y:\n",
    "Xc=7.5\n",
    "Yc=7.5\n",
    "\n",
    "# Distorted coords x,y:\n",
    "Xd=8.0\n",
    "Yd=8.0\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 116,
   "metadata": {},
   "outputs": [],
   "source": [
    "#helper expression for r^2:\n",
    "r2=(Xd-Xc)*(Xd-Xc)+(Yd-Yc)*(Yd-Yc)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$X_u=X_c+\\frac{(X_d-X_c)}{(1+K_1 r^2+K_2 r^4)}$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$Y_u=Y_c+\\frac{(Y_d-Y_c)}{(1+K_1 r^2+K_2 r^4)}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 117,
   "metadata": {},
   "outputs": [],
   "source": [
    "X_undistorted = Xc+(Xd-Xc)/(1+K1*r2+K2*r2*r2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 118,
   "metadata": {},
   "outputs": [],
   "source": [
    "Y_undistorted = Yc+(Yd-Yc)/(1+K1*r2+K2*r2*r2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 119,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Undistorted X,Y:  7.976190476190476 7.976190476190476\n"
     ]
    }
   ],
   "source": [
    "print(\"Undistorted X,Y: \",X_undistorted,Y_undistorted)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# Radial Distortion Model - MDIS NAC\n",
    "### The following code is from the ik kernel for the MDIS NAC instrument.  The NAC distortion\n",
    "### was determined by fitting data from a simulation of the NAC's optical behavior to a 3rd order\n",
    "### Taylor series expansion.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 120,
   "metadata": {},
   "outputs": [],
   "source": [
    "odtx=[1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0]\n",
    "odty=[0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0]\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### [dx,dy]=distortMe(ux,uy):\n",
    "### Takes in undistorted focal plane coordinates and returns distorted coordinates [dx,dy]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 121,
   "metadata": {},
   "outputs": [],
   "source": [
    "def distortMe(ux,uy,dtx,dty):\n",
    "    \n",
    "    f=[1,ux,uy,ux**2,ux*uy,uy**2,ux**3,uy*ux**2,ux*uy**2,uy**3]\n",
    "    dx=0.0\n",
    "    dy=0.0\n",
    "    for i in range(len(f)):\n",
    "        dx = dx+f[i]*dtx[i]\n",
    "        dy = dy+f[i]*dty[i]    \n",
    "    return [dx,dy]\n",
    "        \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### [ux,uy]=undistortMe(dx,dy):\n",
    "### Computes undistored focal plane (ux,uy) coordinates given distorted focal plane \n",
    "### coordinates using the Newton-Raphson Method for root finding a system of equations:\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$f_1(x_1,x_2,\\cdots,x_n) = f_1(\\textbf{x}) = \\textbf{0}$$\n",
    "$$f_2(x_1,x_2,\\cdots,x_n) = f_2(\\textbf{x}) = \\textbf{0}$$\n",
    "$$\\vdots$$\n",
    "$$f_n(x_1,x_2,\\cdots,x_n) = f_n(\\textbf{x}) = \\textbf{0}$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### To solve consider Taylor series expansion of f about x to first order:\n",
    "\n",
    "$$f(\\textbf{x} +\\delta \\textbf{x} ) = f_i(\\textbf{x}) + \\sum_{j=0}^n\\frac{\\partial f_i}{\\partial x_j}\\delta x_j +O(\\delta x ^2),i = 1,\\cdots,n$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ignore the $O(\\delta x^2)$ terms and let $f(x+\\delta x) = 0 $ (ie, $x+\\delta x$ is the root we are seeking).  Then:\n",
    "### $$-f_i(\\textbf{x}) = \\sum_{j=0}^n\\frac{\\partial f_i}{\\partial x_j}\\delta x_j, j=0,\\cdots ,n$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Therefore:  $\\delta x_j = -f(\\textbf{x})J_f^{-1}(\\textbf(x)$ ($J$ = the Jacobian of $f$)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### And finally:\n",
    "$$ \\textbf{x} \\Leftarrow \\textbf{x}+ \\delta(\\textbf{x}) = \\textbf{x} - J_f^{-1}(\\textbf{x})f(\\textbf{x})$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### In code:\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 122,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Jacobian(x,y,odtx,odty):\n",
    "    d_dx=[0.0,1.0,0.0,2*x,y,0.0,3*x*x,2*x*y,y*y,0.0]\n",
    "    d_dy=[0.0,0.0,1.0,0.0,x,2*y,0.0,x**2,2*x*y,3*y**2]\n",
    "    \n",
    "    Jxx=0.0;\n",
    "    Jxy = 0.0;\n",
    "    Jyx= 0.0;\n",
    "    Jyy = 0.0;\n",
    "    \n",
    "    for i in range(len(d_dx)):\n",
    "        Jxx = Jxx+d_dx[i]*odtx[i]\n",
    "        Jxy = Jxy+d_dy[i]*odtx[i]\n",
    "        Jyx = Jyx + d_dx[i]*odty[i]\n",
    "        Jyy = Jyy + d_dy[i]*odty[i] \n",
    "    \n",
    "    return [Jxx,Jxy,Jyx,Jyy]\n",
    "\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 123,
   "metadata": {},
   "outputs": [],
   "source": [
    "def undistortMe(dx,dy,odtx,odty):\n",
    "    epsilon =1.4e-7\n",
    "    maxIts = 60\n",
    "    #initial guess\n",
    "    x = dx\n",
    "    y = dy\n",
    "    [fx,fy]= distortMe(x,y,odtx,odty)\n",
    "    for i in range(maxIts):        \n",
    "        [fx,fy]=distortMe(x,y,odtx,odty)\n",
    "        fx=dx-fx\n",
    "        fy = dy-fy\n",
    "        [Jxx,Jxy,Jyx,Jyy] = Jacobian(x,y,odtx,odty)\n",
    "        determinant = Jxx*Jyy-Jxy*Jyx\n",
    "       \n",
    "        if (abs(determinant) < 1e-7):\n",
    "            print(\"Jacobian is singular.\")\n",
    "            print(determinant)\n",
    "            break;\n",
    "        else:\n",
    "            x = x+(Jyy*fx-Jxy*fy)/determinant\n",
    "            y = y+(Jxx*fy-Jyx*fx)/determinant\n",
    "        if ( (abs(fx)+abs(fy)) <= epsilon):\n",
    "            return [x,y]\n",
    "    return [dx,dy]    \n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 124,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[232.5, 121.0, 128.5, 240.0]\n",
      "[908.5, 963.75]\n",
      "[7.499999999999999, 7.5]\n"
     ]
    }
   ],
   "source": [
    "distortion = distortMe(7.5,7.5,odtx,odty)\n",
    "undistorted = undistortMe(distortion[0],distortion[1],odtx,odty)\n",
    "J=Jacobian(2.5,2.5,odtx,odty)\n",
    "\n",
    "print([Jxx,Jxy,Jyx,Jyy])\n",
    "print(distortion)\n",
    "print (undistorted)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
